#include <contour_detection.hpp>
//image is the binary image i have

namespace ohe_framework{

unsigned int findContours(ImageMatrixUchar &image, std::vector<Blob> &contours)
{
    unsigned short  n=0;
    unsigned short rows = image.getRows();
    unsigned short cols = image.getCols();
    unsigned short flag, flag_1;
    unsigned short max_x,max_y,min_x,min_y;
    unsigned char* data = new unsigned char[rows*cols];// = image.getDataPointer();
    unsigned int widthstep = image.getWidthstep();
    for(int i =0;i<rows;i++)
    {
        memcpy(&data[i*cols],&((image.getDataPointer())[i*widthstep]),cols);
    }
    for(unsigned short i=0;i<cols;i++)
    {
        data[(rows-1)*cols+i]=data[i]=0;
        data[(rows-2)*cols+i]=data[cols+i]=0;
    }
    for(unsigned short i=0;i<rows;i++)
    {
        data[i*cols+cols-1]=data[i*cols]=0;
        data[i*cols+cols-2]=data[i*cols+1]=0;
    }
    int i=0;
    char orientation;
    Point start,current,precurrent;
    start.x=0;
    start.y=0;
    contours.clear();
    int iy=0,ix=0;
    while (1/*n<20*/)
    {
        flag=0;
        // bug may iy and ix should be used outside
        for(;iy<rows;iy++)
        {
            for(;ix<cols;ix++)
            {
                if(data[iy*cols+ix]>0)
                {
                    start.y=iy;
                    start.x=ix;
                    flag=1;
                    break;
                }
            }
            if(flag)
                break;
        }
        if(flag==0)
            break;
        current.y=start.y;
        current.x=start.x;
        precurrent.y=start.y;
        precurrent.x=start.x-1;
        orientation='E';
        max_x=0;
        max_y=0;
        min_x=cols;
        min_y=rows;
        Blob blob;
        //std::cout<<"New Contour"<<std::endl;
        do
        {
            flag_1=0;
            switch(orientation)
            {
                case 'N':
                    if(data[precurrent.y*cols+(precurrent.x-1)])
                    {
                        current.y=precurrent.y;
                        current.x=precurrent.x-1;
                        blob.addPoint(current);
                        orientation='W';
                    }
                    else 
                        if(data[(precurrent.y-1)*cols+(precurrent.x-1)])
                        {
                            current.y=precurrent.y-1;
                            current.x=precurrent.x-1;
                            blob.addPoint(current);
                            precurrent.y=precurrent.y;
                            precurrent.x=precurrent.x-1;
                            orientation='N';
                        }
                        else 
                            if(data[(precurrent.y-2)*cols+(precurrent.x-1)])
                            {
                                current.y=precurrent.y-2;
                                current.x=precurrent.x-1;
                                blob.addPoint(current);
                                precurrent.y=precurrent.y-1;
                                precurrent.x=precurrent.x-1;
                                orientation='N';
                            }    
                            else 
                                if(data[(precurrent.y-2)*cols+(precurrent.x)])
                                {
                                    current.y=precurrent.y-2;
                                    current.x=precurrent.x;
                                    blob.addPoint(current);
                                    precurrent.y=precurrent.y-2;
                                    precurrent.x=precurrent.x-1;
                                    orientation='E';
                                }   
                                else 
                                    if(data[(precurrent.y-2)*cols+(precurrent.x+1)])
                                    {
                                        current.y=precurrent.y-2;
                                        current.x=precurrent.x+1;
                                        blob.addPoint(current);
                                        precurrent.y=precurrent.y-2;
                                        precurrent.x=precurrent.x;
                                        orientation='E';
                                    }
                                    else 
                                        if(data[(precurrent.y-1)*cols+(precurrent.x+1)])
                                        {
                                            current.y=precurrent.y-1;
                                            current.x=precurrent.x+1;
                                            blob.addPoint(current);
                                            precurrent.y=precurrent.y-2;
                                            precurrent.x=precurrent.x+1;
                                            orientation='S';
                                        }
                                        else 
                                            if(data[(precurrent.y)*cols+(precurrent.x+1)])
                                            {
                                                current.y=precurrent.y;
                                                current.x=precurrent.x+1;
                                                blob.addPoint(current);
                                                precurrent.y=precurrent.y-1;
                                                precurrent.x=precurrent.x+1;
                                                orientation='S';
                                            }
                                            else
                                                flag_1=1;
                    break;
                case 'S':
                    if(data[precurrent.y*cols+(precurrent.x+1)])
                    {
                        current.y=precurrent.y;
                        current.x=precurrent.x+1;
                        blob.addPoint(current);
                        orientation='E';
                    }
                    else 
                        if(data[(precurrent.y+1)*cols+(precurrent.x+1)])
                        {
                            current.y=precurrent.y+1;
                            current.x=precurrent.x+1;
                            blob.addPoint(current);
                            precurrent.y=precurrent.y;
                            precurrent.x=precurrent.x+1;
                            orientation='S';
                        }
                        else 
                            if(data[(precurrent.y+2)*cols+(precurrent.x+1)])
                            {
                                current.y=precurrent.y+2;
                                current.x=precurrent.x+1;
                                blob.addPoint(current);
                                precurrent.y=precurrent.y+1;
                                precurrent.x=precurrent.x+1;
                                orientation='S';
                            }
                            else 
                                if(data[(precurrent.y+2)*cols+(precurrent.x)])
                                {
                                    current.y=precurrent.y+2;
                                    current.x=precurrent.x;
                                    blob.addPoint(current);
                                    precurrent.y=precurrent.y+2;
                                    precurrent.x=precurrent.x+1;
                                    orientation='W';
                                }
                                else 
                                    if(data[(precurrent.y+2)*cols+(precurrent.x-1)])
                                    {
                                        current.y=precurrent.y+2;
                                        current.x=precurrent.x-1;
                                        blob.addPoint(current);
                                        precurrent.y=precurrent.y+2;
                                        precurrent.x=precurrent.x;
                                        orientation='W';
                                    }
                                    else 
                                        if(data[(precurrent.y+1)*cols+(precurrent.x-1)])
                                        {
                                            current.y=precurrent.y+1;
                                            current.x=precurrent.x-1;
                                            blob.addPoint(current);
                                            precurrent.y=precurrent.y+2;
                                            precurrent.x=precurrent.x-1;
                                            orientation='N';
                                        }
                                        else 
                                            if(data[(precurrent.y)*cols+(precurrent.x-1)])
                                            {
                                                current.y=precurrent.y;
                                                current.x=precurrent.x-1;
                                                blob.addPoint(current);
                                                precurrent.y=precurrent.y+1;
                                                precurrent.x=precurrent.x-1;
                                                orientation='N';
                                            }
                                            else
                                                flag_1=1;
                    break;
                case 'E':
                    if(data[(precurrent.y-1)*cols+(precurrent.x)])
                    {
                        current.y=precurrent.y-1;
                        current.x=precurrent.x;
                        blob.addPoint(current);
                        orientation='N';
                    }
                    else 
                        if(data[(precurrent.y-1)*cols+(precurrent.x+1)])
                        { 
                            current.y=precurrent.y-1;
                            current.x=precurrent.x+1;
                            blob.addPoint(current);
                            precurrent.y=precurrent.y-1;
                            precurrent.x=precurrent.x;
                            orientation='E';
                        }
                        else 
                            if(data[(precurrent.y-1)*cols+(precurrent.x+2)])
                            {                
                                current.y=precurrent.y-1;
                                current.x=precurrent.x+2;
                                blob.addPoint(current);
                                precurrent.y=precurrent.y-1;
                                precurrent.x=precurrent.x+1;
                                orientation='E';
                            }
                            else 
                                if(data[(precurrent.y)*cols+(precurrent.x+2)])
                                {               
                                    current.y=precurrent.y;
                                    current.x=precurrent.x+2;
                                    blob.addPoint(current);
                                    precurrent.y=precurrent.y-1;
                                    precurrent.x=precurrent.x+2;
                                    orientation='S';
                                }
                                else 
                                    if(data[(precurrent.y+1)*cols+(precurrent.x+2)])
                                    {            
                                        current.y=precurrent.y+1;
                                        current.x=precurrent.x+2;
                                        blob.addPoint(current);
                                        precurrent.y=precurrent.y;
                                        precurrent.x=precurrent.x+2;
                                        orientation='S';
                                    }
                                    else 
                                        if(data[(precurrent.y+1)*cols+(precurrent.x+1)])
                                        {
                                            current.y=precurrent.y+1;
                                            current.x=precurrent.x+1;
                                            blob.addPoint(current);
                                            precurrent.y=precurrent.y+1;
                                            precurrent.x=precurrent.x+2;
                                            orientation='W';
                                        }
                                        else if(data[(precurrent.y+1)*cols+(precurrent.x)])
                                            {
                                                current.y=precurrent.y+1;
                                                current.x=precurrent.x;
                                                blob.addPoint(current);
                                                precurrent.y=precurrent.y+1;
                                                precurrent.x=precurrent.x+1;
                                                orientation='W';
                                            }
                                            else
                                                flag_1=1;
                    break;
                case 'W':
                    if(data[(precurrent.y+1)*cols+(precurrent.x)])
                    {                            
                        current.y=precurrent.y+1;
                        current.x=precurrent.x;
                        blob.addPoint(current);
                        orientation='S';
                    }
                    else 
                        if(data[(precurrent.y+1)*cols+(precurrent.x-1)])
                        {                        
                            current.y=precurrent.y+1;
                            current.x=precurrent.x-1;
                            blob.addPoint(current);
                            precurrent.y=precurrent.y+1;
                            precurrent.x=precurrent.x;
                            orientation='W';
                        }
                        else 
                            if(data[(precurrent.y+1)*cols+(precurrent.x-2)])
                            {
                                current.y=precurrent.y+1;
                                current.x=precurrent.x-2;
                                blob.addPoint(current);
                                precurrent.y=precurrent.y+1;
                                precurrent.x=precurrent.x-1;
                                orientation='W';
                            }
                            else 
                                if(data[(precurrent.y)*cols+(precurrent.x-2)])
                                {
                                    current.y=precurrent.y;
                                    current.x=precurrent.x-2;
                                    blob.addPoint(current);
                                    precurrent.y=precurrent.y+1;
                                    precurrent.x=precurrent.x-2;
                                    orientation='N';
                                }
                                else 
                                    if(data[(precurrent.y-1)*cols+(precurrent.x-2)])
                                    {    
                                        current.y=precurrent.y-1;
                                        current.x=precurrent.x-2;
                                        blob.addPoint(current);
                                        precurrent.y=precurrent.y;
                                        precurrent.x=precurrent.x-2;
                                        orientation='N';
                                    }
                                    else 
                                        if(data[(precurrent.y-1)*cols+(precurrent.x-1)])
                                        {
                                            current.y=precurrent.y-1;
                                            current.x=precurrent.x-1;
                                            blob.addPoint(current);
                                            precurrent.y=precurrent.y-1;
                                            precurrent.x=precurrent.x-2;
                                            orientation='E';
                                        }
                                        else 
                                            if(data[(precurrent.y-1)*cols+(precurrent.x)])
                                            {
                                                current.y=precurrent.y-1;
                                                current.x=precurrent.x;
                                                blob.addPoint(current);
                                                precurrent.y=precurrent.y-1;
                                                precurrent.x=precurrent.x-1;
                                                orientation='E';
                                            }
                                            else
                                                flag_1=1;
                    break;
            }
            if(flag_1==1)
            {
                break;
            }
            if(current.y>max_y)         
                max_y=current.y;
            if(current.x>max_x)
                max_x=current.x;
            if(current.y<min_y)
                min_y=current.y;
            if(current.x<min_x)
                min_x=current.x;
            
        }while((current.y!=start.y)||(current.x!=start.x));
        if(blob.getNoOfPoints()>5)
        {
            contours.push_back(blob);
        }
        for(short l=min_y-1;l<=max_y;l++)
        {
            for(short m=min_x-1;m<=max_x;m++)
            {
                data[l*cols+m]=0;
            }
        }
        n++;
        //std::cout<<"Points found : "<<n<<std::endl;
    }
    delete[] data;
    return n;
}
bool plotContoursBoundary(ImageMatrixUchar& image, Blob &contour,
                  unsigned char red, unsigned char green,
                  unsigned char blue)
{
    if(image.isMatrixFree())
    {
        printErrorMessage("Can't put contour on image as it doesn't have memory");
        return false;
    }
    else
    {
        if(image.getNoOfChannels()==3)
        {
            Point start(0,0),end(0,0);
            contour.getBoundingRectangle(start,end);
            std::vector<Point>* points = contour.getPoints();
            if((end.x<image.getCols())&&(end.y<image.getRows()))
            {
                unsigned char* data = image.getDataPointer();
                unsigned int widthstep = image.getWidthstep();
                for(int i=0;i<points->size();i++)
                {
                    data[(*points)[i].y*widthstep+(*points)[i].x*3+0]=blue;
                    data[(*points)[i].y*widthstep+(*points)[i].x*3+1]=green;
                    data[(*points)[i].y*widthstep+(*points)[i].x*3+2]=red;
                }
                //cv::waitKey(0);
            }
            else
            {
                printErrorMessage("Image is not suffienct to plot contours");
                return false;
            }
        }
        else
        {
            printErrorMessage("No color image given");
            return false;
        }
    }
}
bool plotContoursFill(ImageMatrixUchar& image, Blob &contour,
                  unsigned char red, unsigned char green,
                  unsigned char blue)
{
    if(image.isMatrixFree())
    {
        printErrorMessage("Can't put contour on image as it doesn't have memory");
        return false;
    }
    else
    {
        if(image.getNoOfChannels()==3)
        {
            Point start,end;
            contour.getBoundingRectangle(start,end);
            std::vector<Point>* points = contour.getPoints();
            if((end.x<image.getCols())&&(end.y<image.getRows()))
            {
                unsigned char* data = image.getDataPointer();
                unsigned int widthstep = image.getWidthstep();
                for(int j =start.y;j<=end.y;j++)
                {
                    int x_min = end.x;
                    int x_max = start.x;
                    for(int i=0;i<points->size();i++)
                    {
                        if((*points)[i].y==j)
                        {
                            if((*points)[i].x>x_max)
                            {
                                x_max =(*points)[i].x; 
                            }
                            if((*points)[i].x<x_min)
                            {
                                x_min =(*points)[i].x; 
                            }
                        }
                    }
                    for(int i=x_min;i<=x_max;i++)
                    {
                        data[j*widthstep+i*3+0]=blue;
                        data[j*widthstep+i*3+1]=green;
                        data[j*widthstep+i*3+2]=red;
                    }
                }
            }
            else
            {
                printErrorMessage("Image is not suffienct to plot contours");
                return false;
            }
        }
        else
        {
            printErrorMessage("No color image given");
            return false;
        }
    }
}

Line::Line()
{
    start.y = start.x = 0;
    end.y = end.x = 0;
}
Line::~Line()
{
    
}

bool Line::drawLine(ImageMatrixUchar& image,
                    unsigned char red, unsigned char green,
                    unsigned char blue)
{
    if(image.isMatrixFree())
    {
        printErrorMessage("Can't put line on image as it doesn't have memory");
        return false;
    }
    else
    {
        if(image.getNoOfChannels()==3)
        {
            if((end.x<image.getCols())&&(end.y<image.getRows())&&(end.x>0)&&(end.y>0)
                &&(start.x<image.getCols())&&(start.y<image.getRows())&&(start.x>0)&&(start.y>0)    )
            {
                //if(start.x>0&&start.y>0)
                {
                    int x_min,x_max,y_min,y_max;
                    unsigned char* data = image.getDataPointer();
                    unsigned int widthstep = image.getWidthstep();
                    if(end.x>start.x)
                    {
                        x_min = start.x;
                        x_max = end.x;
                    }
                    else
                    {
                        x_min = end.x;
                        x_max = start.x;
                    }
                    if(end.y>start.y)
                    {
                        y_min = start.y;
                        y_max = end.y;
                    }
                    else
                    {
                        y_min = end.y;
                        y_max = start.y;
                    }
                    if((x_max-x_min)!=0)
                    {
                        
                        double slope = ((double)(y_max-y_min))/(x_max-x_min);
                        for(int i = x_min+1;i<=x_max;i++)
                        {
                            int j = y_min +(i-x_min)*slope;
                            //for(int j = y_min; j <y_min +(i-x_min)*slope;j++)
                            if(j<=y_max)
                            {
                                data[j*widthstep+i*3+0]=blue;
                                data[j*widthstep+i*3+1]=green;
                                data[j*widthstep+i*3+2]=red;
                            }
                            //y_min += (i-x_min)*slope;
                            //std::cout<<"i "<<i<<" j "<<j<<std::endl;
                        }
//                        std::cout<<"start recieved" << start.x<<" " <<start.y<<std::endl;
//                        //std::cout<<"slope "<<slope<<std::endl;
//                        std::cout<<"end recieved" << end.x<<" " <<end.y<<std::endl;
                    }
                    else
                    {    
                        for(int j = y_min;j<=y_max;j++)
                        {
                            int i = x_min;
                            data[j*widthstep+i*3+0]=blue;
                            data[j*widthstep+i*3+1]=green;
                            data[j*widthstep+i*3+2]=red;
                        }
                    }
                }
            }  
        }
        else
        {
            printErrorMessage("No color image given");
            return false;
        }
    }
    //std::cout<<"drawing lines"<<std::endl;
}

}// end namespace ohe_framework
